#ifndef _BUSINESS_H_
#define _BUSINESS_H_
#include "rads_database.hpp"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream> // 用于字符串流
#include <vector>  // 用于整数数组
#include <iostream>
#include <sqlite3.h>
#include <ctime>
#include <string>
#include <iomanip>
#include <cstdlib>
#include <sys/wait.h>
#include "ros/time.h" // 用于ros::Time
#include <tf/transform_listener.h>
#include <thread>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <fstream>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include "rads/rads_scmdret.h"
#include "rads/rads_cmd.h"
#include "rads/rads_acks.h"
#include "rads/rads_version.h"
#include "rads/rads_versions.h"
#include "rads/rads_log.h"
#include <chrono>
#include <mutex>
#include <cmath>
#include <stdexcept>
#include <functional>
#include <std_msgs/Float32.h>
#include "nav_msgs/Odometry.h"
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/Imu.h>
#include "serial/serial.h"

using namespace std;
ros::Publisher sradsack_pub_; //发布响应网络信息（主要用于响应网络指令，上传地图，上传路线点等）话题名《sradsack》
ros::Publisher sradsdrive;// 发布控制车辆的信息,话题名《sradsdriveackermann》
ros::Publisher initialposepub;//发布地图重定位,话题名《initialpose》
// ros::Publisher sradsversionpub;//发布OTA版本，话题名《sradsversion》
// ros::Publisher sradslogpub;// 发布日志话题，话题名《sradslog》
rads::rads_scmdret rads_scmdretdata;
// 全局变量和同步机制
using ThreadFunc_createline = std::function<void(int, int, string)>;//定义《带图带线路线点》传参
using ThreadFunc_createline_all = std::function<void(int)>;//定义《马踏飞燕》传参
using ThreadFunc_createline_goback = std::function<void(int)>;//定义《跟推踩点》传参
using ThreadFunc_createline_goback_play = std::function<void(int)>;//定义《返航》传参


std::atomic<bool> should_run(false);//带图带线路线点
std::atomic<bool> should_run_all(false);//马踏飞燕
std::atomic<bool> should_run_goback(false);//跟推踩点
std::atomic<bool> should_run_goback_play(false);//返航
std::atomic<bool> stopRadarThread(false); // 自动驾驶


std::thread thread_instance_createline;//定义《带图带线路线点》线程
std::thread thread_instance_createline_all;//定义《马踏飞燕》线程
std::thread thread_instance_createline_goback;//定义《跟推踩点》线程
std::thread thread_instance_createline_goback_play;//定义《返航》线程

ThreadFunc_createline thread_func_createline;//带图带线路线点
ThreadFunc_createline_all thread_func_createline_all;//马踏飞燕
ThreadFunc_createline_goback thread_func_createline_goback;//跟推踩点
ThreadFunc_createline_goback_play thread_func_createline_goback_play;//返航

std::thread startinginit;

int Stop_lidar,stop_car;
int line_stop_flag=0;
int key_1,key_2,key_3,key_4,key_5,key_6,key_7,key_8;
int ladar_1,ladar_2,ladar_3,ladar_4,ladar_5,ladar_6,ladar_7,ladar_8;
int FLLR = 0,LFRFLRRR = 0;
int heartbeat = 0;

//rtk&&雷达所有数据
typedef struct
{
  int point_count;
  double x;
  double y;
  double z;
  double ox;
  double oy;
  double oz;
  double ow;
  double yaw;
  double matching_rate;
  double longitude;
  double latitude;
  double altitude;
  double heading;
  double groud_speed;
  double posi_flag;
  double head_flag;
  double ext;
  double flag;
  double rtk_x;
  double rtk_y;
  double rtk_z;
  double f_x;
  double f_y;
  double f_z;
  double ld_heading;
}locate_information;  
locate_information lidar_rtk;
//雷达返航使用的数据
typedef struct
{
  double x;
  double y;
  double z;
  double ox;
  double oy;
  double oz;
  double ow;
  double ld_heading;
}lidar_goback;
lidar_goback lidar_gobackmsg;
//外装的IMU
typedef struct
{
  double x;
  double y;
  double z;
}imu;
imu imu_msg;
//sradsack应答式发布
typedef struct
{
  string vehicle_id;
  string pro_nema;
  int cmd;
  int cmd_opt;
  int ext1;
  int ext2;
  int ack;
  int map_id;
  int path_id;
  string map_path;
  string progress;
}sradsack_fast;




void publishacks(ros::Publisher pub, const sradsack_fast& sradsack_fast_data)
{
  rads::rads_acks msg;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = "sradsack_map"; // or whatever frame_id is appropriate
  msg.pro_nema = sradsack_fast_data.pro_nema;
  msg.cmd = sradsack_fast_data.cmd;
  msg.cmd_opt = sradsack_fast_data.cmd_opt;
  msg.ext1 = sradsack_fast_data.ext1;
  msg.ext2 = sradsack_fast_data.ext2;
  msg.ack = sradsack_fast_data.ack;
  msg.map_path = sradsack_fast_data.map_path;
  msg.map_id = sradsack_fast_data.map_id;
  msg.path_id = sradsack_fast_data.path_id;
  pub.publish(msg);
}
// 计算两个坐标点之间的距离
double distance_count(double aim_x, double aim_y, double pos_x, double pos_y)
{
  double distance = sqrt((aim_x - pos_x) * (aim_x - pos_x) + (aim_y - pos_y) * (aim_y - pos_y));
  return distance;
}


// 重定位
void Repositioning(int map_id)
{
double minNDT_gl =70.0;                           // 匹配度
  geometry_msgs::PoseWithCovarianceStamped pose_msg;
  pose_msg.header.stamp = ros::Time::now();
  pose_msg.header.frame_id = "map";
  pose_msg.pose.pose.position.x = 0;
  pose_msg.pose.pose.position.y = 0;
  pose_msg.pose.pose.position.z = 0;
  pose_msg.pose.pose.orientation.x = 0;
  pose_msg.pose.pose.orientation.y = 0;
  pose_msg.pose.pose.orientation.z = 0;
  pose_msg.pose.pose.orientation.w = 0;
  initialposepub.publish(pose_msg);

}
// 切换地图
bool map_ck(int map_id)
{

  char command[256];
  snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./map_loader.sh %d &", (int)map_id);
  int result = system(command);
  //Repositioning(map_id);
  return result;
}



/*任意点导航 start*/
// 计算两个坐标点之间的距离
double DistanceCount(AxisData &data1, AxisData &data2)
{
  double distance = sqrt(pow((data1.x - data2.x), 2) + pow((data1.y - data2.y), 2));
  return distance;
}
// 创建节点 value点的索引值
ListNode *CreateNode(int value)
{
  return new ListNode(value);
}
// 向前插入节点
void InsertNodeToHead(ListNode *&head, int value)
{
  ListNode *newNode = CreateNode(value);
  newNode->next = head;
  head = newNode;
}
// 打印链表
void PrintList(ListNode *head)
{
  Path_test[0] = 0;
  while (head != nullptr)
  {
    std::cout << head->value << " -> ";
    Path_test[0]++;
    Path_test[Path_test[0]] = head->value;
    head = head->next;
  }
  std::cout << "null" << std::endl;
}
// 输入一个链表，返回int数组，点位总数（第一位）和点位索引（从索引1开始往后表示）
int *CopyList(ListNode *head)
{
  static int copy_data[1000]; // 需要返回的值和点位
  copy_data[0] = 0;
  while (head != nullptr)
  {
    copy_data[0]++;
    copy_data[copy_data[0]] = head->value;
    head = head->next;
  }
  return copy_data;
}
// 输入坐标x，y 匹配最近点的索引
int FindPoint(AxisData &data,std::vector<point_all> &points)
{
  double minDistance = std::numeric_limits<double>::max(); // 初始化一个最大距离用于计算邻近点比较器
  for (auto i = 0; i != points.size(); i++)
  {
    AxisData tmp;
    tmp.x = points[i].x;
    tmp.y = points[i].y;
    double distance = DistanceCount(data, tmp);
    if (distance < minDistance)
    {
      minDistance = distance, Current_NUM = i;
    }
  }
  // path_pub.publish(path);
  return Current_NUM;
}
// 找当前索引点最近距离点
int *FindPointNum(int point_num,std::vector<point_all> &points)
{
  static int point_data[5]; // 需要返回的值和点位
  point_data[0] = 0;
  for (auto i = 0; i != points.size(); i++)
  {

    AxisData aim(Nav_x[point_num], Nav_y[point_num]);
    AxisData pos(Nav_x[i], Nav_y[i]);
    double distance = DistanceCount(aim, pos);
    if (distance < 3.5 && distance > 1.5)
    {
      point_data[0]++;
      point_data[point_data[0]] = i;
    }
  }
  if (point_data[0] > 0)
  {
    Nav_x[point_num] = 99999;
    Nav_y[point_num] = 99999;
  }
  return point_data;
}
// 计算起点和目标点的规划路径
vector<int16_t> PathPlaning(AxisData &curPose, AxisData &aimPose,std::vector<point_all> &points)
{
  int jump_flag = 0;
  int Line_num = 0;
  int Line_count = 1;
  vector<int16_t> res;
  // 起点 给出坐标值，输出该坐标的标号//当前坐标
  int pos_num = FindPoint(curPose,points);
  // 目标点 给出坐标值，输出该坐标的标号//目标
  int aim_num = FindPoint(aimPose,points);

  if (pos_num == aim_num)
  {

    res.push_back(aim_num);
  }
  else
  {

    for (int i = 0; i < 180; ++i)
    {
      lists[i] = nullptr;
    }
    InsertNodeToHead(lists[0], aim_num); // 把30条路线的头插入 第一个点，把初始搜索点插入到第0条线路的第一个
    int count20 = 0;
    //********************         导航算法如下           *************************
    for (auto i = 0; i != points.size(); i++)
    {
      if (jump_flag == 1)
      {
        break;
      }
      int Line_data = Line_count;
      for (int j = 0; j < Line_data; ++j) // 有多少根线，对每根线操作   J 代表第几根线
      {
        count20++;
        if (jump_flag == 1)
        {
          break;
        }

        int *p = FindPointNum(lists[j]->value,points); // 把每个线路的最后一个点位查找附近点数量//    如果复制链表，需要在此处下复制
        if (*(p) == 1)
        {
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
        }
        else if (*(p) == 2)
        {
          int *R = CopyList(lists[j]);          // 输出链表的个数和数字  lists[j + Line_count - 1] = nullptr;
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
          for (size_t f = *(R); f > 0; f--)
          {
            InsertNodeToHead(lists[Line_count], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
          }
          InsertNodeToHead(lists[Line_count], *(p + 2)); // 代表的是第几个连表，代表的是插入的数据值
          Line_count = Line_count + 1;
        }
        else if (*(p) == 3)
        {
          int *R = CopyList(lists[j]);          // 输出链表的个数和数字  lists[j + Line_count - 1] = nullptr;
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
          for (size_t f = *(R); f > 0; f--)
          {
            InsertNodeToHead(lists[Line_count], *(R + f));     // 代表的是第几个连表，代表的是插入的数据值
            InsertNodeToHead(lists[Line_count + 1], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
          }
          InsertNodeToHead(lists[Line_count], *(p + 2));     // 代表的是第几个连表，代表的是插入的数据值
          InsertNodeToHead(lists[Line_count + 1], *(p + 3)); // 代表的是第几个连表，代表的是插入的数据值
          Line_count = Line_count + 2;
        }
        else if (*(p) == 4)
        {
          int *R = CopyList(lists[j]);          // 输出链表的个数和数字  lists[j + Line_count - 1] = nullptr;
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
          for (size_t f = *(R); f > 0; f--)
          {
            InsertNodeToHead(lists[Line_count], *(R + f));     // 代表的是第几个连表，代表的是插入的数据值
            InsertNodeToHead(lists[Line_count + 1], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
            InsertNodeToHead(lists[Line_count + 2], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
          }
          InsertNodeToHead(lists[Line_count], *(p + 2));     // 代表的是第几个连表，代表的是插入的数据值
          InsertNodeToHead(lists[Line_count + 1], *(p + 3)); // 代表的是第几个连表，代表的是插入的数据值
          InsertNodeToHead(lists[Line_count + 2], *(p + 3)); // 代表的是第几个连表，代表的是插入的数据值
          Line_count = Line_count + 3;
        }

        for (size_t x = 0; x < *p; x++)
        {
          if (*(p + 1 + x) == pos_num)
          {
            jump_flag = 1;
            Line_num = j;
            break;
          }
        }
      }
    }
    if (Line_num >= 0)
    {
      ListNode *head = lists[Line_num];
      // PrintList(head);
      while (head != nullptr)
      {
        res.push_back(head->value);
        head = head->next;
      }
    }
  }

  return res;
}
std::vector<int16_t> getPath_anypoint(double x1, double y1, double x2, double y2,std::vector<point_all> &points)
{
  AxisData currentPosition(x1, y1);
  AxisData targetPosition(x2, y2);
  return PathPlaning(currentPosition, targetPosition,points);
}
/*任意点导航 end*/


// 角度归一化函数
double normalize_angle(double angle) {
  while (angle < -M_PI) angle += 2 * M_PI;
  while (angle >= M_PI) angle -= 2 * M_PI;
  return angle;
}


// 吸附点查找函数
int findSnappedWaypoint(const std::vector<car>& pathPoints) {
  int snapped_index = -1;
  double min_distance = std::numeric_limits<double>::max();
  const double MAX_SEARCH_DISTANCE = 10.0;  // 距离阈值

  for (size_t i = 0; i < pathPoints.size(); ++i) {
      const car& point = pathPoints[i];
      
      // 1. 计算距离
      double distance = distance_count(lidar_rtk.x, lidar_rtk.y, point.x, point.y);
      // 新增：距离超过阈值则跳过该点
      if (distance > MAX_SEARCH_DISTANCE) {
        continue;  // 直接检查下一个点
    }
      // 2. 计算航向角差
      double point_heading = atan2(point.y - lidar_rtk.y, point.x - lidar_rtk.x);
      double heading_diff = fabs(normalize_angle(point_heading - lidar_rtk.ld_heading));
      
      // 3. 筛选条件：距离最近 + 前方点（航向差<90°）
      if (distance < min_distance && heading_diff < M_PI_2) {
          min_distance = distance;
          snapped_index = i;
      }
  }
  return snapped_index;
}



double calculate_NDT_JDC(const car& target_point) {
// 计算差值
double delta_x,delta_y,aim_angle,angle_diff;
if (RTK_LIKE == 1) {
delta_x = lidar_rtk.x - target_point.x;
delta_y = lidar_rtk.y - target_point.y;
aim_angle = atan2(delta_x, delta_y) * 57.3;
}
if (RTK_LIKE == 2) {
double delta_x = lidar_rtk.rtk_x - target_point.rtk_x;
double delta_y = lidar_rtk.rtk_y - target_point.rtk_y;
double aim_angle = atan2(delta_x, delta_y) * 57.3+270;
}

// 规范化角度到0-360度范围
aim_angle = fmod(aim_angle, 360.0);
if (aim_angle < 0) {
aim_angle += 360.0;
}

// 计算角度差并规范化到-180到180度范围
if(RTK_LIKE==1){
angle_diff = aim_angle - lidar_rtk.ld_heading;
}
if(RTK_LIKE==2){
angle_diff = aim_angle - lidar_rtk.heading;
}
angle_diff = fmod(angle_diff + 180.0, 360.0) - 180.0;

return angle_diff;
}


// 定义常量PI
const double PIS = 3.14159265358979323846;
// 将角度转换为弧度
double degreesToRadians(double degrees) {
  return degrees * PIS / 180.0;
}
// 计算两个天线中间位置的坐标
void calculateMidPoint(double x1, double y1, double yawDegrees, double baseline, double &xMid, double &yMid) {
  // 将航向角度转换为弧度
  double yawRadians = degreesToRadians(yawDegrees);
  // 计算第二个天线的坐标
  double deltaX = cos(yawRadians) * baseline;
  double deltaY = sin(yawRadians) * baseline;
  double x2 = x1 + deltaX;
  double y2 = y1 + deltaY;
  // 计算中间位置的坐标
  xMid = (x1 + x2) / 2.0;
  yMid = (y1 + y2) / 2.0;
}


void pubdrive(int brake, int gear, int steering, int target_speed)
{
    heartbeat++;
    rads_scmdretdata.heartbeat = heartbeat;
    rads_scmdretdata.len = 25;    // len
    rads_scmdretdata.brake = brake; // 制动值0-100
    rads_scmdretdata.gear = gear;    // 0=P，1=N，2=D，3=R，4=O；
    rads_scmdretdata.steering = steering;
    rads_scmdretdata.target_speed = target_speed; // 速度：单位：KM/h
    rads_scmdretdata.header.stamp = ros::Time::now();
    rads_scmdretdata.header.frame_id = "sradsdriveackermann";
    sradsdrive.publish(rads_scmdretdata);
}
// 计算当前车辆剩余里程，Lidar_msg当前车辆的坐标，data，是所有的点集，startIdx是下个目标点下标
double calculateTotalDistance(int startIdx,const std::vector<car>& pathPoints) {
    double totalDistance = 0.0;
    if (startIdx >= pathPoints.size()) {
        return -1.0; // 或者其他表示错误的特殊值
    }
    const car& fpoint = pathPoints[startIdx];

    double fx = fpoint.x; // 将字符串转换为double
    double fy = fpoint.y; // 将字符串转换为double
    totalDistance += distance_count(lidar_rtk.x, lidar_rtk.y, fx, fy);
    for (size_t i = startIdx; i < pathPoints.size()-1; ++i) {
       const car& point1 = pathPoints[i];
       const car& point2 = pathPoints[i+1];
        double x1 = point1.x; // 将字符串转换为double
        double y1 = point1.y; // 将字符串转换为double
        double x2 = point2.x; // 将字符串转换为double
        double y2 = point2.y; // 将字符串转换为double
        totalDistance += distance_count(x1,y1,x2,y2);
    }
    return totalDistance;
}

void publishSradsackProgress(int i,const std::vector<car>& pathPoints,int speed) {
    rads::rads_acks msg;
    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = "sradsack_map";
    msg.ack = 3;
    
    // Calculate progress percentage
    double newpercentage = 100 - (static_cast<double>(pathPoints.size() - i) / pathPoints.size() * 100.0);
    int newbaifenbi = static_cast<int>(newpercentage + 0.5);  // rounded percentage
    
    // Calculate distance and time
    double totalDistance = calculateTotalDistance(i,pathPoints);
    int newlicheng = static_cast<int>(totalDistance);
    double newspeedInMPS = speed / 10.0 / 3.6;
    double newseconds = totalDistance / newspeedInMPS;
    int newduoshaomiao = static_cast<int>(newseconds);
    
    // Format progress string
    msg.progress = std::to_string(newbaifenbi) + "," + 
                   std::to_string(newduoshaomiao) + "," + 
                   std::to_string(newlicheng);
    
    // Publish conditionally
   
        // std::cout << "base_timestamp: " << base_timestamp() << ","
        //           << "totalDistance:" << "-------------" << totalDistance << std::endl;
        sradsack_pub_.publish(msg);
    
}

//踩点处理
double normalized_heading_diff(double a, double b) {
    double diff = fabs(a - b);
    return (diff > 180.0) ? 360.0 - diff : diff;  // 处理环绕
}


#endif